Multivariate KF Examples
Example 9 – vehicle location estimation
We design a six-dimensional Kalman Filter without control input. That is, we estimate a vehicle’s location on the XY plane. The vehicle has an on-board location sensor that reports X and Y coordinates of the system. We assume constant acceleration dynamics.
Kalman Filter equations
There is no control input so: \(\textbf{u} = 0\)
The state extrapolation equation: \[\hat{\textbf{x}}_{n+1, n} = \textbf{F} \hat{\textbf{x}}_{n, n}\]
The system state \(x_{n}\) is defined by: \[ \textbf{x}_{n} = \begin{bmatrix} x_{n} \\ \dot{x}_{n} \\ \ddot{x}_{n} \\ y_{n} \\ \dot{y}_{n} \\ \ddot{y}_{n} \end{bmatrix} \]
So the extrapolated vehicle state for time n + 1 can be described by:
\(\begin{array}{ccc} \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \\ \hat{\ddot{x}}_{n+1,n} \\ \hat{y}_{n+1,n} \\ \hat{\dot{y}}_{n+1,n} \\ \hat{\ddot{y}}_{n+1,n} \end{bmatrix} = & \begin{bmatrix} 1 & \Delta t & 0.5 \Delta t^2 & 0 & 0 & 0\\ 0 & 1 & \Delta t & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0.5 \Delta t^2\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} & \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix} \end{array}\)
The covariance extrapolation equation:
The estimate covariance matrix is, assuming that the estimation error in \(X\) and \(Y\) axes are not correlated: \[ P = \begin{bmatrix} p_{x} & p_{x\dot{x}} & p_{x\ddot{x}} & 0 & 0 & 0 \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ 0 & 0 & 0 & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{bmatrix} \]
The process noise matrix, assuming a discrete noise model: \[\begin{align*} Q = & \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \\ \end{bmatrix} \\ &= \sigma^2_{a} \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\ \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\ 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \\ \end{bmatrix} \end{align*}\]
The measurement equation:
The measurement provides us only \(X\) and \(Y\) coordinates of the vehicle: \[ z_{n} = \begin{bmatrix} x_{n, measured} \\ y_{n, measured} \end{bmatrix} \]
The dimension of \(z_{n}\) is \(2×1\) and the dimension of \(x_{n}\) is \(6×1\). Therefore the dimension of the observation matrix \(H\) shall be \(2 × 6\). \[ \textbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \]
The measurement uncertainty, assuming that the \(x\) and \(y\) measurements are uncorrelated: \[ \textbf{R}_{n} = \begin{bmatrix} \sigma^2_{x_{m}} & 0 \\ 0 & \sigma^2_{y_{m}} \end{bmatrix} \]
The subscript \(m\) is for measurement uncertainty, but for this example we assume a constant measurement uncertainty: \[\textbf{R}_{1} = ... = \textbf{R}_{n}=\textbf{R}\]
So we have defined all the necessary blocks for the Kalman Gain, The state update equation and The covariance update equation.
Numerical example
Let us assume a vehicle moving in a straight line in the X direction with a constant velocity. After traveling 400 meters the vehicle turns right, with a turning radius of 300 meters. During the turning maneuver, the vehicle experiences acceleration due to the circular motion (angular acceleration).
# The measurements period (s)
deltaT = 1
# The random acceleration standard deviation (m/s^2)
sigma_a = 0.2
# The measurement error standard deviation (m)
sigma_xm = 3
sigma_ym = 3
# The state transition matrix F would be
vec1 <- c(1, 1, 0.5, 0, 0, 0)
vec2 <- c(0, 1, 1, 0, 0, 0)
vec3 <- c(0, 0, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1, 1, 0.5)
vec5 <- c(0, 0, 0, 0, 1, 1)
vec6 <- c(0, 0, 0, 0, 0, 1)
transMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6)
# The process noise matrix Q would be
vec1 <- c(1/4, 1/2, 1/2, 0, 0, 0)
vec2 <- c(1/2, 1, 1, 0, 0, 0)
vec3 <- c(1/2, 1, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1/4, 1/2, 1/2)
vec5 <- c(0, 0, 0, 1/2, 1, 1)
vec6 <- c(0, 0, 0, 1/2, 1, 1)
processNoiseMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6) * 0.2^2
# The measurement covariance R would be
R <- rbind(c(20, 0), c(0, 20))
# Observation matrix
H <- rbind(c(1, 0, 0, 0, 0, 0), c(0, 0, 0, 1, 0, 0))# The set of 35 noisy measurements
x <- c(301.5, 298.23, 297.83, 300.42, 301.94, 299.5, 305.98, 301.25, 299.73, 299.2, 298.62, 301.84, 299.6, 295.3, 299.3, 301.95, 296.3, 295.11, 295.12, 289.9, 283.51, 276.42, 264.22, 250.25, 236.66, 217.47, 199.75, 179.7, 160, 140.92, 113.53, 93.68, 69.71, 45.93, 20.87)
y <- c(-401.46, -375.44, -346.15, -320.2, -300.08, -274.12, -253.45, -226.4, -200.65, -171.62, -152.11, -125.19, -93.4, -74.79, -49.12, -28.73, 2.99, 25.65, 49.86, 72.87, 96.34, 120.4, 144.69, 168.06, 184.99, 205.11, 221.82, 238.3, 253.02, 267.19, 270.71, 285.86, 288.48, 292.9, 298.77)
zn <- cbind(x,y)Iteration Zero
# Initialization
x00 <- c(0, 0, 0, 0, 0, 0) # as we do not know the vehicle location
# Since is a guess, we set a very high estimate uncertainty
# The high estimate uncertainty results in a high Kalman Gain by giving a high weight to the measurement.
P00 <- diag(c(500, 500, 500, 500, 500, 500))Prediction
## [,1]
## vec1 0
## vec2 0
## vec3 0
## vec4 0
## vec5 0
## vec6 0
## vec1 vec2 vec3 vec4 vec5 vec6
## vec1 1125.01 750.02 250.02 0.00 0.00 0.00
## vec2 750.02 1000.04 500.04 0.00 0.00 0.00
## vec3 250.02 500.04 500.04 0.00 0.00 0.00
## vec4 0.00 0.00 0.00 1125.01 750.02 250.02
## vec5 0.00 0.00 0.00 750.02 1000.04 500.04
## vec6 0.00 0.00 0.00 250.02 500.04 500.04
First Iteration
## [1] 301.50 -401.46
# Step 2 - Update
# The Kalman Gain calculation:
k1 <- P10 %*% t(H) %*% solve(H %*% P10 %*% t(H) + R); k1## [,1] [,2]
## vec1 0.9825329 0.0000000
## vec2 0.6550336 0.0000000
## vec3 0.2183562 0.0000000
## vec4 0.0000000 0.9825329
## vec5 0.0000000 0.6550336
## vec6 0.0000000 0.2183562
Therefore, we can see that the Kalman Gain for a position is 0.9921, which means that the weight of the first measurement is significantly higher than the weight of the estimation; that is the weight of the estimation is negligible.
## [,1]
## vec1 296.23367
## vec2 197.49262
## vec3 65.83439
## vec4 -394.44766
## vec5 -262.96978
## vec6 -87.66127
# Update the current estimate uncertainty
P11 <- (diag(6) - k1 %*% H) %*% P10 %*% t(diag(6) - k1 %*% H) + k1 %*% R %*% t(k1); P11## vec1 vec2 vec3 vec4 vec5 vec6
## vec1 19.650658 13.10067 4.367123 0.000000 0.00000 0.000000
## vec2 13.100672 508.75171 336.268504 0.000000 0.00000 0.000000
## vec3 4.367123 336.26850 445.446590 0.000000 0.00000 0.000000
## vec4 0.000000 0.00000 0.000000 19.650658 13.10067 4.367123
## vec5 0.000000 0.00000 0.000000 13.100672 508.75171 336.268504
## vec6 0.000000 0.00000 0.000000 4.367123 336.26850 445.446590
## [,1]
## vec1 526.64349
## vec2 263.32701
## vec3 65.83439
## vec4 -701.24808
## vec5 -350.63105
## vec6 -87.66127
## vec1 vec2 vec3 vec4 vec5 vec6
## vec1 1006.6110 1253.3656 563.3789 0.0000 0.0000 0.0000
## vec2 1253.3656 1626.7753 781.7551 0.0000 0.0000 0.0000
## vec3 563.3789 781.7551 445.4866 0.0000 0.0000 0.0000
## vec4 0.0000 0.0000 0.0000 1006.6110 1253.3656 563.3789
## vec5 0.0000 0.0000 0.0000 1253.3656 1626.7753 781.7551
## vec6 0.0000 0.0000 0.0000 563.3789 781.7551 445.4866
Generalization
multiKF <- function(transMat, processNoiseMat, H, R, iniX, iniP, zn){
# Initialization
x <- list(iniX)
p <- list(iniP)
k <- list(0)
# Prediction
x <- append(x, list(transMat %*% x[[1]]))
p <- append(p, list(transMat %*% p[[1]] %*% t(transMat) + processNoiseMat))
for (i in 2:dim(zn)[1]){
# Measure
z <- c(zn[i-1, 1], zn[i-1, 2])
# Update
k <- append(k, list(p[[length(p)]] %*% t(H) %*% solve(H %*% p[[length(p)]] %*% t(H) + R)))
# Estimate the current state
x <- append(x, list(x[[length(x)]] + k[[i]] %*% (z - H %*% x[[length(x)]])))
# Update the current estimate uncertainty
p <- append(p, list((diag(dim(k[[i]])[1]) - k[[i]] %*% H) %*% p[[length(p)]] %*% t(diag(dim(k[[i]])[1]) - k[[i]] %*% H) + k[[i]] %*% R %*% t(k[[i]])))
# Predict
x <- append(x, list(transMat %*% x[[length(x)]]))
p <- append(p, list(transMat %*% p[[length(p)]] %*% t(transMat) + processNoiseMat))
}
return(list(x, p, k))
}
result <- multiKF(transMat, processNoiseMat, H, R, x00, P00, zn)
x <- result[[1]]
p <- result[[2]]
k <- result[[3]]Visualization
Vehicle X-axis velocity
Since the absolute velocity of the mobile is 26, the true values of
the velocity in the x axis will be: for the first 15s \(0\) and for the other 20s remaining \(-26\sin\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)
# Take the velocity estimates in the x axis
estimatesX <- c()
for (i in seq(3, 70, by = 2)){
estimatesX = append(estimatesX, list(x[[i]][2]))
}
trueValX <- c(rep(0, 15), -26*sin(((16:34 - 15) * pi) / 38))
# Dataframe with all the data
dataVx <- data.frame(
recta = c(1:34),
estimX = unlist(estimatesX),
trueVal = trueValX
)# Plot to compare the true value, measured values and estimates
plotVx <- ggplot(dataVx, aes(x = recta)) +
geom_path(aes(y = trueVal, color = "True values")) +
geom_point(aes(y = trueVal, color = "True values", shape = "True values")) +
geom_path(aes(y = estimX, color = "Estimates")) +
geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
labs(title = "Vehicle X-axis velocity",
x = "Time(s)",
y = "Vx(m/s)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")
ggplotly(plotVx) %>% config(displayModeBar = FALSE)Vehicle Y-axis velocity
Since the absolute velocity of the mobile is 26, the true values of
the velocity in the x axis will be: for the first 15s \(26\) and for the other 20s remaining \(\cos\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)
# Take the velocity estimates in the y axis
estimatesY <- c()
for (i in seq(3, 70, by = 2)){
estimatesY = append(estimatesY, list(x[[i]][5]))
}
# trueValY <- c(rep(1, 15), -cos(((16:34 - 400) * pi) / 600))
trueValY <- c(rep(26, 15), 26*cos(((16:34 - 15) * pi) / 38))
# Dataframe with all the data
dataVy <- data.frame(
recta = c(1:34),
estimY = unlist(estimatesY),
trueVal = trueValY
)# Plot to compare the true value, measured values and estimates
plotVy <- ggplot(dataVy, aes(x = recta)) +
geom_path(aes(y = trueVal, color = "True values")) +
geom_point(aes(y = trueVal, color = "True values", shape = "True values")) +
geom_path(aes(y = estimY, color = "Estimates")) +
geom_point(aes(y = estimY, color = "Estimates", shape = "Estimates")) +
labs(title = "Vehicle Y-axis velocity",
x = "Time(s)",
y = "Vy(m/s)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")
ggplotly(plotVy) %>% config(displayModeBar = FALSE)Vehicle Position
The true values of the position in the x axis will be: for the first
15s \(300\) and for the other 20s
remaining \(300\cos\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)
The true values of the position in the x axis will be: the first
value \(-400\) and then for the next
14s \(-400 + 26*t\) and for the other
20s remaining \(300\sin\theta(t)\)
\(\theta(t) = \frac{(t-15)
\pi}{38}\)
# True values
trueValposX <- c(rep(300, 15), 300 * cos(((16:35 - 15) * pi) / 38))
trueValposY <- c(-400, -400 + 26*1:13, 0, 300 * sin(((16:35 - 15) * pi) / 38))
# Take the estimates in the x and y axis
estimatesX <- c()
estimatesY <- c()
for (i in seq(3, 70, by = 2)){
estimatesX = append(estimatesX, list(x[[i]][1]))
estimatesY = append(estimatesY, list(x[[i]][4]))
}
estimatesX = append(estimatesX, list(19.3))
estimatesY = append(estimatesY, list(297.79))
# Confidence ellipse for the probability of 95%:
scaleFactor <- sqrt(-2*log(1 - 0.95))
muValX <- c()
muValY <- c()
phiVal <- c()
kaVal <- c()
kbVal <- c()
for (i in seq(3, 70, by = 2)){
# Get the covariance matrix for each estimation
sigma2 <- rbind(c(p[[i]][1,1], 0),
c(0, p[[i]][4,4]))
# Get the eigen values and eigen vectors
res <- eigen(sigma2)
# θ = arctan(vy/vx)
phiVal <- append(phiVal, list(atan(res$vectors[2,1] / res$vectors[1,1])))
# k * the highest eigenvalue square root
kaVal <- append(kaVal, list(scaleFactor * sqrt(res$values[1])))
# k * the second eigenvalue square root
kbVal <- append(kbVal, list(scaleFactor * sqrt(res$values[2])))
# Centre of the ellipse, each estimation in the x and y axis
muValX <- append(muValX, list(c(x[[i]][1])))
muValY <- append(muValY, list(c(x[[i]][4])))
}
muValX <- append(muValX, c(19.3))
muValY <- append(muValY, c(297.79))
phiVal <- append(phiVal, c(-1.570796))
kaVal <- append(kaVal, c(scaleFactor * sqrt(5)))
kbVal <- append(kbVal, c(scaleFactor * sqrt(5)))# Dataframe with all the data
data <- data.frame(
estimX = unlist(estimatesX), # only the estimates
estimY = unlist(estimatesY),
measurements.x = zn[,1],
measurements.y = zn[,2],
muValX <- unlist(muValX),
muValY <- unlist(muValY),
phiVal <- unlist(phiVal),
kaVal <- unlist(kaVal),
kbVal <- unlist(kbVal),
trueValX = trueValposX,
trueValY = trueValposY
)# Plot to compare the true value, measured values and estimates
ggplot(data) +
geom_path(aes(x = estimX, y = estimY, color = "Estimates")) +
geom_point(aes(x = estimX, y = estimY, color = "Estimates", shape = "Estimates")) +
geom_path(aes(x = measurements.x, y = measurements.y, color = "Measurements")) +
geom_point(aes(x = measurements.x, y = measurements.y, color = "Measurements", shape = "Measurements")) +
geom_ellipse(aes(x0 = muValX, y0 = muValY, a = kaVal, b = kbVal, angle = phiVal, color = "Interval", shape = 'Interval'), fill = 'yellow', alpha = 0.2) +
geom_path(aes(x = trueValposX, y = trueValposY, color = "True values")) +
geom_point(aes(x = trueValposX, y = trueValposY, color = "True values", shape = "True values")) +
coord_fixed() +
labs(title = "Vehicle Position",
x = "X(m)",
y = "Y(m)",
color = "") +
xlim(c(0.00001, 310)) +
scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green", "Interval" = 'yellow'), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18, 'Interval' = 1), name = "")The circles on the plot represent the 95% confidence ellipse. Since the x and y axes’ measurement errors are equal, the confidence ellipse is a circle.
Our Kalman Filter is designed for a constant acceleration model. Nevertheless, due to a properly chosen \(\sigma^2_{a}\) parameter it succeeds in tracking maneuvering vehicle, that is, the vehicle experiencing acceleration due to the circular motion - angular acceleration.
Example 10 – rocket altitude estimation
In this example, we will we estimate the altitude of a rocket, that is, we will design a two-dimensional Kalman Filter with a control input. We assume constant acceleration dynamics.
The rocket is equipped with an on-board altimeter that provides altitude measurements. In addition, the rocket is equipped with an accelerometer that measures the rocket’s acceleration, this serves as a control input to the Kalman Filter.
Accelerometers don’t sense gravity. Thus, we need to subtract the gravitational acceleration constant g from each accelerometer measurement.The accelerometer measurement at time step n is: \(a_{n} = \ddot{x} - g + \epsilon\) Where: \(\ddot{x}\) is the actual acceleration of the object (the second derivative of the object position), \(g\) is the gravitational acceleration constant (\(g\) = −9.8\(\frac{m}{s^2}\)) and \(\epsilon\) is the accelerometer measurement error.
Kalman Filter equations
For the state extrapolation equation: \(\begin{array}{ccc} \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \end{bmatrix} = & \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix} & \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \end{bmatrix} + & \begin{bmatrix} 0.5\Delta t^2\\ \Delta t \end{bmatrix} & (a_{n} + g) \end{array}\)
For the covariance extrapolation equation: \[ P = \begin{bmatrix} p_{x} & p_{x\dot{x}} \\ p_{\dot{x}x} & p_{\dot{x}} \\ \end{bmatrix} \]
The process noise matrix, assuming a discrete noise model: \[\begin{align*} Q = & \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}}\\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}}\\ \end{bmatrix} \\ &= \epsilon^2 \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\ \frac{\Delta t^3}{2} & \Delta t^2 \\ \end{bmatrix} \end{align*}\]
The accelerometer error \(v\) is much lower than the system’s random acceleration; therefore, we use \(\epsilon^2\) as a multiplier of the process noise matrix. It makes our estimation uncertainty much lower.
In this example, the acceleration is handled by control input (therefore, it is not part of \(F\) matrix, and the \(Q\) matrix is \(2 × 2\)). Without an external control input, the process matrix would be \(3 × 3\).
The measurement equation:
The measurement provides only the altitude of the rocket: \[ z_{n} = \begin{bmatrix} x_{n, measured} \end{bmatrix} \]
The dimension of \(z_{n}\) is \(1 × 1\) and the dimension of \(x_{n}\) is \(2 × 1\), so the dimension of the observation matrix \(\textbf{H}\) is \(1 × 2\).
\[ \textbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} \]
The measurement uncertainty: \[ \textbf{R}_{n} = \begin{bmatrix} \sigma^2_{x_{m}} \end{bmatrix} \]
The subscript \(m\) means the “measurement”, but for this example we assume a constant measurement uncertainty: \[\textbf{R}_{1} = ... = \textbf{R}_{n}=\textbf{R}\]
So we have defined all the necessary blocks for the Kalman Gain, The state update equation and The covariance update equation.
Numerical example
Let us assume a vertically boosting rocket with constant acceleration. The rocket is equipped with an altimeter that provides altitude measurements and an accelerometer that serves as a control input.
# The measurements period: ∆t = 0.25s
# The rocket acceleration: x = 30 m/s^2
# The altimeter measurement error standard deviation: σ_xm = 20m
# The accelerometer measurement error standard deviation: ϵ = 0.1
# The state transition matrix F would be:
transF <- rbind(c(1, 0.25),
c(0, 1))
# The control matrix G would be:
G <- c(0.5*0.25**2, 0.25)
# The process noise matrix Q would be:
Q <- 0.1**2 * rbind(c(0.25**4/4, 0.25**3/2),
c(0.25**3/2, 0.25**2))
# The measurement uncertainty R would be:
R <- c(20**2)
# The observation matrix H
H <- cbind(1, 0)# The set of 30 noisy measurements of the altitude hn and acceleration an:
hn <- c(6.43, 1.3, 39.43, 45.89, 41.44, 48.7, 78.06, 80.08, 61.77, 75.15, 110.39, 127.83, 158.75, 156.55, 213.32, 229.82, 262.8, 297.57, 335.69, 367.92, 377.19, 411.18, 460.7, 468.39, 553.9, 583.97, 655.15, 723.09, 736.85, 787.22)
an <- c(39.81, 39.67, 39.81, 39.84, 40.05, 39.85, 39.78, 39.65, 39.67, 39.78, 39.59, 39.87, 39.85, 39.59, 39.84, 39.9, 39.63, 39.59, 39.76, 39.79, 39.73, 39.93, 39.83, 39.85, 39.94, 39.86, 39.76, 39.86, 39.74, 39.94)Initialization
# We don’t know the rocket’s location; we set the initial position and velocity to 0
x00 <- c(0,0)
# We assume
u0 = 0
# Since our initial state vector is a guess, we set a very high estimate uncertainty
p00 <- rbind(c(500, 0), c(0, 500))Prediction
## [,1]
## [1,] 0
## [2,] 0
## [,1] [,2]
## [1,] 531.2500 125.0001
## [2,] 125.0001 500.0006
First iteration
## [,1]
## [1,] 0.5704698
## [2,] 0.1342283
## [,1]
## [1,] 3.6681208
## [2,] 0.8630878
p11 <- (diag(dim(k1)[1]) - k1 %*% H) %*% p10 %*% t(diag(dim(k1)[1]) - k1 %*% H) + k1 %*% R %*% t(k1); p11## [,1] [,2]
## [1,] 228.18792 53.69131
## [2,] 53.69131 483.22208
# Predict, the prediction uncertainty is still very high
x21 <- transF %*% x11 + G * (an[1] - 9.8); x21## [,1]
## [1,] 4.821705
## [2,] 8.365588
## [,1] [,2]
## [1,] 285.2350 174.4969
## [2,] 174.4969 483.2227
Generalization
multiKF2 <- function(transMat, processNoiseMat, H, R, G, iniX, iniP, hn, an){
# Initialization
x <- list(iniX)
u <- list(0)
p <- list(iniP)
k <- list(0)
# Prediction
x <- append(x, list(transMat %*% x[[1]] + G * u[[1]]))
p <- append(p, list(transMat %*% p[[1]] %*% t(transMat) + processNoiseMat))
for (i in 2:length(hn)){
# Update
k <- append(k, list(p[[length(p)]] %*% t(H) %*% solve(H %*% p[[length(p)]] %*% t(H) + R)))
# Estimate the current state
x <- append(x, list(x[[length(x)]] + k[[i]] %*% (hn[i-1] - H %*% x[[length(x)]])))
# Update the current estimate uncertainty
p <- append(p, list((diag(dim(k[[i]])[1]) - k[[i]] %*% H) %*% p[[length(p)]] %*% t(diag(dim(k[[i]])[1]) - k[[i]] %*% H) + k[[i]] %*% R %*% t(k[[i]])))
# Predict
x <- append(x, list(transMat %*% x[[length(x)]] + G * (an[i-1] - 9.8)))
p <- append(p, list(transMat %*% p[[length(p)]] %*% t(transMat) + processNoiseMat))
}
return(list(x, p, k))
}
result <- multiKF2(transF, Q, H, R, G, x00, p00, hn, an)
x <- result[[1]]
p <- result[[2]]
k <- result[[3]]Visualization
Rocket Altitude
The true values of a vertically boosting rocket with constant acceleration \(30 m/s^2\) are calculated by \(x=ut+\frac{1}{2}at^2\) so assuming the rocket starts from rest \(u = 0\):
# True values for the first 7.25 seconds for each 0.25s
acceleration <- 30
trueValposX <- 0.5 * acceleration * seq(0, 7.25, by = 0.25)^2
# Take the estimates in the x axis
estimatesX <- c()
for (i in seq(3, 60, by = 2)){
estimatesX = append(estimatesX, list(x[[i]][1]))
}
estimatesX = append(estimatesX, list(797.07))
# Take the estimates in the x and y axis
estimSigmaX <- c()
for (i in seq(3, 60, by = 2)){
estimSigmaX = append(estimSigmaX, list(p[[i]][1,1]))
}
estimSigmaX = append(estimSigmaX, list(49.3))# Dataframe with all the data
data <- data.frame(
time = c(1:30),
estimX = unlist(estimatesX),
measurements.x = hn,
# the 95% confidence interval is given by [-1.96 sqrt(V); +1.96 sqrt(V)]
interMax = unlist(estimatesX) + 1.96 * sqrt(unlist(estimSigmaX)),
interMin = unlist(estimatesX) - 1.96 * sqrt(unlist(estimSigmaX)),
trueValX = trueValposX
)# Plot to compare the true value, measured values and estimates
plot1 <- ggplot(data, aes(x = time)) +
geom_path(aes(y = measurements.x, color = "Measurements")) +
geom_point(aes(y = measurements.x, color = "Measurements", shape = "Measurements")) +
geom_path(aes(y = trueValposX, color = "True values")) +
geom_point(aes(y = trueValposX, color = "True values", shape = "True values")) +
geom_path(aes(y = estimX, color = "Estimates")) +
geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
geom_line(aes(y = interMax, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
geom_line(aes(y = interMin, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
geom_ribbon(aes(ymin = interMin, ymax = interMax), fill = "yellow", alpha = 0.2) +
labs(title = "Rocket Altitude",
x = "Time(s)",
y = "Altitude(m)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green", "95% confidence interval" = 'yellow'), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18, '95% confidence interval' = 1), name = "")
ggplotly(plot1) %>% config(displayModeBar = FALSE)We can see a good KF tracking performance and convergence.
Rocket Velocity
The true values of a vertically boosting rocket with constant acceleration \(30 m/s^2\) are calculated by \(v=u+at\) so assuming the rocket starts from rest \(u = 0\):
# True values for the first 7.25 seconds for each 0.25s
acceleration <- 30
trueValposV <- acceleration * seq(0, 7.25, by = 0.25)
# Take the estimates
estimatesV <- c()
for (i in seq(3, 60, by = 2)){
estimatesV = append(estimatesV, list(x[[i]][2]))
}
estimatesV = append(estimatesV, list(215.7))
# Take the estimates in the x and y axis
estimSigmaV <- c()
for (i in seq(3, 60, by = 2)){
estimSigmaV = append(estimSigmaV, list(p[[i]][2,2]))
}
estimSigmaV = append(estimSigmaV, list(2.6))# Dataframe with all the data
dataV <- data.frame(
time = c(1:30),
estimX = unlist(estimatesV),
measurements.x = an,
# the 95% confidence interval is given by [-1.96 sqrt(V); +1.96 sqrt(V)]
interMax = unlist(estimatesV) + 1.96 * sqrt(unlist(estimSigmaV)),
interMin = unlist(estimatesV) - 1.96 * sqrt(unlist(estimSigmaV)),
trueValX = trueValposV
)# Plot to compare the true value, measured values and estimates
plot2 <- ggplot(dataV, aes(x = time)) +
geom_path(aes(y = trueValX, color = "True values")) +
geom_point(aes(y = trueValX, color = "True values", shape = "True values")) +
geom_path(aes(y = estimX, color = "Estimates")) +
geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
geom_line(aes(y = interMax, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
geom_line(aes(y = interMin, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
geom_ribbon(aes(ymin = interMin, ymax = interMax), fill = "yellow", alpha = 0.2) +
labs(title = "Rocket Altitude",
x = "Time(s)",
y = "Altitude(m)",
color = "") +
scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green", "95% confidence interval" = 'yellow'), name = "") +
scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18, '95% confidence interval' = 1), name = "")
ggplotly(plot2) %>% config(displayModeBar = FALSE)It takes about 2.5 seconds to converge the estimates to the true rocket velocity. In the beginning, the estimated altitude is influenced by measurements, and it is not aligned well with the true rocket altitude since the measurements are very noisy. But as the KF converges, the noisy measurement has less influence, and the estimated altitude is well aligned with the true altitude.